/******************************************************************************************************
 * Real Time Systems Project 6
 *
 * HCS12 implementation of system to display voltage from QNX using servo motors
 *
 * Author: Sajin George
 *         Philip Rodriguez
 *         Alfred Lee
 *         OLIVER WING
 * 
 *
 * Timer Setup ,Serial Port initialization and printf function from Professor Bullinger's Example Code
 * Revisions: 
 * 
 ******************************************************************************************************/

// system includes
#include <hidef.h>      /* common defines and macros */
#include <stdio.h>      /* Standard I/O Library */

// project includes
#include "types.h"
#include "derivative.h" /* derivative-specific definitions */
#include "sysinit.h"
#include "termio.h"

// maximum value of digital data sent by QNX
#define MAX_EIGHT_BIT_BINARY_VAL	(128)
#define EIGHT_BIT_MAX				(255)

//Used for infinite loops
#define FOREVER						(1)	


// Global variables

// whether to update the positions
// of the servos or not
UINT8 update = FALSE;

//Stores Digital Value converted by QNX A2D
INT8 CharacterBuffer = 0;

// Converts the digital data from QNX to PWM duty cycles
// by scaling the value from the digital data to a point
// between the maximum and minimum positions for each servo
INT8 convertDigitalDataToPWM(UINT8 servoMotorNumber, INT8 digitalData)
{

	INT8 pwmForServo;
	UINT8 pwmDifference = Pos5DutyCycleValue - Pos0DutyCycleValue;
	
	// Determine which servo motor number to work with, since
	// the calculations for each servo are different
	if (servoMotorNumber == Servo1)
	{
		// scale the voltage received from QNX in digital data to a
		// PWM corresponding to a point between positions 0 and 5
		pwmForServo = Pos0DutyCycleValue - ((digitalData * pwmDifference) / MAX_EIGHT_BIT_BINARY_VAL);
		
		// if the voltage value from QNX is positive, servo 1
		// will be at position 0
		if (pwmForServo < Pos0DutyCycleValue)
		{
			pwmForServo = Pos0DutyCycleValue;
		}
	}
	else if (servoMotorNumber == Servo2)
	{
		// scale the voltage received from QNX in digital data to a
		// PWM corresponding to a point between positions 0 and 5
		pwmForServo = Pos5DutyCycleValue - ((digitalData * pwmDifference) / MAX_EIGHT_BIT_BINARY_VAL);
		
		// if the voltage value from QNX is negative, servo 2
		// will be at position 5
		if (pwmForServo > Pos5DutyCycleValue)
		{
			pwmForServo = Pos5DutyCycleValue;
		}
	}
	
	return pwmForServo;
}


// Move the specified servo to a certain position
// based on the PWM duty cycle value passed in
void moveServo(UINT8 servo, INT8 pwmValue)
{

	if(servo == Servo1)
	{
	    //update dutycycle register of servo1
		 PWMDTY0 = pwmValue;
		 
	}
	else if(servo == Servo2)
	{
		//update dutycycle register of servo2
		PWMDTY1 = pwmValue;
		 
	}
}

// Initialize the registers, including
// the timer, PWM register, digital IO ports, etc.
void Initialize(void)
{

	//initialize timer
    (void)InitializeTimer();
    
    //initialize pwm
    InitializePWM();
    
    DDRA = 0xff;//output
    DDRB = 0x00;//input
    DDRE = 0x00;//input
    
}

//Diplay voltage using servos
void displayVoltage(INT8 digitalData)
{
	// convert the digital data into PWM
	// duty cycle values for each servo
	INT8 pwmForServo1, pwmForServo2;
	pwmForServo1 = convertDigitalDataToPWM(Servo1, digitalData);
	pwmForServo2 = convertDigitalDataToPWM(Servo2, digitalData);
	
	// set the servos to move to the specified locations
	// in order to display the voltage received from QNX
	(void)moveServo(Servo1, pwmForServo1);
	(void)moveServo(Servo2, pwmForServo2);
}

INT8 POST_Free(void)
{
	UINT8 index = 0;
	UINT8 results[EIGHT_BIT_MAX];
	UINT8 expectedResults[EIGHT_BIT_MAX];
	UINT8 pwmDifference = Pos5DutyCycleValue - Pos0DutyCycleValue;
	
	//Get expected results
	for(; index < EIGHT_BIT_MAX; index++)
	{
		expectedResults[index] = Pos0DutyCycleValue - ((index * pwmDifference) / MAX_EIGHT_BIT_BINARY_VAL);	
	}

	//Get results from convertDigitalDataToPWM()
	for(; index < EIGHT_BIT_MAX; index++)
	{
		results[index] = convertDigitalDataToPWM(Servo1, index);
	}
	
	for(; index < EIGHT_BIT_MAX; index++)
	{
		if(expectedResults[index]!=results[index])
		{
			return FALSE;
		}
	}
	
	return TRUE;
	
	
}

// Freescale main program starts here
void main(void) 
{
	//button status
	UINT8 ButtonPressed = FALSE;
	UINT8 DAQWorking = FALSE;
	
	//Initilize PWM, TIMEERS, I/Os
	Initialize();

	//not using interrupts
	DisableInterrupt();
	
	if(POST_Free())
	{
			//initialize interrupts
		InitializeInterrupts();
	   	
		// wait for button to be pressed to START
	  	while (ButtonPressed == FALSE)
	  	{
	  		if(!(PORTE & (1 << 2)))
	  		{
	  			ButtonPressed = TRUE;
	  		}	
	  	}
	   	      
	   	// Indicate connection on A[3]
	   	PORTA_BIT3 = 0;	
	   	      	
	    //infinite loop
	    while(FOREVER) 
	   	{
	   		//set PORTA bit zero to start conversion at QNX connect wires to PA0
			PORTA |= (1 << 0);
	      
	      	//wait for conversiton(QNX send a high on PORTE PE0-pin 42)
	      	while (FOREVER)
	      	{
				if(PORTE & (1 << 0))			// successful read
				{
					if (DAQWorking == TRUE)
					{
						DAQWorking = FALSE;
						update = TRUE;// the servo value is updated
						CharacterBuffer = PORTB;//read converted value	
						PORTA_BIT3 = 0;
						break;
					}
				}
				else
				{
					DAQWorking = TRUE;	
				}
	      		
	      		// timeout
				if (TFLG1_C1F)
				{			
					PORTA_BIT3 = 1;
					break;
				}
	      	}
	      	
	      	// QNX finished conversion
	      	
	      	// reset timeout
	  		TCNT = 0;
			TFLG1 = TFLG1_C1F_MASK;		
	      
	      	// reset PORTA bit
			PORTA &=~ (1 << 0);
	      
	     	// Update Servo Position
	    	if (update == TRUE)		
	    	{
	    		// display the voltage received
	    		// from QNX on the servo motors
	    		displayVoltage(CharacterBuffer);
	    		update = FALSE;
	    	}				  
	    }	
	}
 
}